clc;
clear;

%% 声明变量
Cnb        = sym('C',          [3,3], 'real');         % for Gk
Mpvv       = sym('Mpvv',       [3,1], 'real');         % Mpv vector3、for Fk Hk
dt_Cnb     = sym('dt_C',       [3,3], 'real');         % for Fk
dt_fnsf    = sym('dt_fn',      [3,1], 'real');
dt_bataGyr = sym('dt_betaGyr', [3,1], 'real');
dt_bataAcc = sym('dt_betaAcc', [3,1], 'real');

syms dt;
xk = sym('x',[15, 1], 'real');
Pk = sym('P',[15,15], 'real');
Qt = sym('Q',[15,1],  'real');

%% 状态转移矩阵
dt_Ft = sym(zeros(15, 15));
dt_Ft(1:3,10:12)   = -dt_Cnb;
dt_Ft(4:6,1:3)     = askew(dt_fnsf); 
dt_Ft(4:6,13:15)   = dt_Cnb;
dt_Ft(7:9,4:6)     = [0 Mpvv(1) 0; Mpvv(2) 0 0; 0 0 Mpvv(3)]*dt;
dt_Ft(10:12,10:12) = diag(-dt_bataGyr);
dt_Ft(13:15,13:15) = diag(-dt_bataAcc);
Fk = sym(eye(15)) + dt_Ft;          % Fk = I + Ft*dt
disp(Fk);

%% 噪声分配矩阵
Gammak = sym(zeros(15, 15));
Gammak(1:3,1:3)     = -Cnb;
Gammak(4:6,4:6)     =  Cnb;
Gammak(10:12,10:12) =  eye(3);
Gammak(13:15,13:15) =  eye(3);
disp(Gammak);                       % Gammak

%% 时间更新
xkk_1 = Fk * xk;
Pkk_1 = Fk * Pk * Fk' + Gammak * diag(Qt*dt) * Gammak';
Pkk_1 = triu(Pkk_1);
disp(xkk_1);
disp(Pkk_1);

f = matlabFunction(xkk_1,Pkk_1,"File","predict_xkpk","Optimize",true, ...
                   "Vars", {xk, Pk, Qt, Cnb, Mpvv, dt_Cnb, dt_fnsf, dt_bataGyr, dt_bataAcc, dt}, ...
                   "Outputs", {'Xkk_1','Pkk_1'});

%% yaw更新
% syms rk3;
% H = sym(zeros(1,15)); H(3) = 1;
% HPH = H * Pk * H';
% PH = Pk * H'
% Kk = PH / (HPH + rk3)
% Kk * H
% Kk * H * Pk
% (eye(15) - Kk * H) * Pk
% innov_var = H * Pk * H' + Rk;
% disp(H);
% Kk = P*H' / (H*P*H' + Rk); disp(Kk);
% [Kk,recip_of_innov_var]=OptimiseAlgebra(Kk,'recip_of_innov_var');
% disp(Kk);
% disp(recip_of_innov_var);